/*
 * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved.
 *
 * See the NOTICE file(s) distributed with this work for additional
 * information regarding copyright ownership.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0
 *
 * SPDX-License-Identifier: EPL-2.0
 *
 * Contact: mosaic@fokus.fraunhofer.de
 */

package org.eclipse.mosaic.lib.util;

import org.eclipse.mosaic.lib.math.MatrixElementOrder;
import org.eclipse.mosaic.lib.math.Vector3d;
import org.eclipse.mosaic.lib.spatial.PointCloud;
import org.eclipse.mosaic.lib.spatial.RotationMatrix;

import java.io.ByteArrayInputStream;
import java.io.ByteArrayOutputStream;
import java.io.DataInput;
import java.io.DataInputStream;
import java.io.DataOutput;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

public class PointCloudSerialization {

    private static final Vector3d TMP_VECTOR = new Vector3d();

    /**
     * Creates a byte array from a {@link PointCloud} object (serialization), to be used for transport or storage.
     * Use {@link #fromByteArray(byte[])} to deserialize the array back into a {@link PointCloud} object.
     *
     * @param pointCloud the {@link PointCloud} object to serialize into a byte array.
     * @return a byte array to store/transport the {@link PointCloud} object.
     */
    public static byte[] toByteArray(PointCloud pointCloud) {
        if (pointCloud == null) {
            return null;
        }
        try (ByteArrayOutputStream bos = new ByteArrayOutputStream()) {
            DataOutput out = new DataOutputStream(bos);
            writePointCloud(pointCloud, out);
            return bos.toByteArray();
        } catch (IOException e) {
            return new byte[0];
        }
    }

    /**
     * Creates a {@link PointCloud} from a (valid) sequence of bytes. The input byte array should be created beforehand
     * using {@link #toByteArray}. If deserialization does not succeed, this method returns {@code null}.
     *
     * @param bytes the list of bytes previously generated by serializing a {@link PointCloud}
     * @return the deserialized {@link PointCloud} object
     */
    public static PointCloud fromByteArray(byte[] bytes) {
        if (bytes.length == 0) {
            return null;
        }

        try (ByteArrayInputStream bos = new ByteArrayInputStream(bytes)) {
            DataInput in = new DataInputStream(bos);
            return readPointCloud(in);
        } catch (IOException e) {
            return null;
        }
    }

    private static void writePointCloud(PointCloud pointCloud, DataOutput out) throws IOException {
        out.writeLong(pointCloud.getCreationTime());
        writeVector3d(pointCloud.getOrigin(), out);
        writeRotationMatrix(pointCloud.getOrientation(), out);
        if (pointCloud.getReferenceFormat() == PointCloud.PointReference.ABSOLUTE) {
            out.writeBoolean(true);
            out.writeInt(pointCloud.getAbsoluteEndPoints().size());
            for (PointCloud.Point point : pointCloud.getAbsoluteEndPoints()) {
                writePoint(point, out);
            }
        } else {
            out.writeBoolean(false);
            out.writeInt(pointCloud.getRelativeEndPoints().size());
            for (PointCloud.Point point : pointCloud.getRelativeEndPoints()) {
                writePoint(point, out);
            }
        }
    }

    private static void writePoint(PointCloud.Point point, DataOutput dataOutput) throws IOException {
        writeVector3d(point, dataOutput);
        dataOutput.writeFloat(point.getDistance());
        dataOutput.writeByte(point.getHitType());
    }

    private static void writeVector3d(Vector3d vector3d, DataOutput dataOutput) throws IOException {
        writeCoordinate(vector3d.x, dataOutput);
        writeCoordinate(vector3d.y, dataOutput);
        writeCoordinate(vector3d.z, dataOutput);
    }

    private static void writeCoordinate(double v, DataOutput dataOutput) throws IOException {
        dataOutput.writeFloat((float) v);
    }

    private static void writeRotationMatrix(RotationMatrix rotation, DataOutput dataOutput) throws IOException {
        double[] values = rotation.getAsDoubleArray(new double[9], MatrixElementOrder.COLUMN_MAJOR);
        for (double value : values) {
            dataOutput.writeDouble(value);
        }
    }

    private static PointCloud readPointCloud(DataInput in) throws IOException {
        long timeStamp = in.readLong();
        Vector3d origin = readVector3d(in, new Vector3d());
        RotationMatrix orientation = readRotation(in);
        boolean storeAbsolutePoints = in.readBoolean();
        int size = in.readInt();
        List<PointCloud.Point> endPoints = new ArrayList<>(size);
        for (int i = 0; i < size; i++) {
            endPoints.add(readPoint(in));
        }
        if (storeAbsolutePoints) {
            return new PointCloud(timeStamp, origin, orientation, endPoints, PointCloud.PointReference.ABSOLUTE);
        } else {
            return new PointCloud(timeStamp, origin, orientation, endPoints, PointCloud.PointReference.RELATIVE);
        }
    }

    private static PointCloud.Point readPoint(DataInput dataInput) throws IOException {
        return new PointCloud.Point(readVector3d(dataInput, TMP_VECTOR), dataInput.readFloat(), dataInput.readByte());
    }

    private static Vector3d readVector3d(DataInput dataInput, Vector3d result) throws IOException {
        return result.set(readCoordinate(dataInput), readCoordinate(dataInput), readCoordinate(dataInput));
    }

    private static double readCoordinate(DataInput dataInput) throws IOException {
        return dataInput.readFloat();
    }

    private static RotationMatrix readRotation(DataInput dataInput) throws IOException {
        double[] values = new double[9];
        for (int i = 0; i < values.length; i++) {
            values[i] = dataInput.readDouble();
        }
        return new RotationMatrix().set(values, MatrixElementOrder.COLUMN_MAJOR);
    }
}
